/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Journal generator!
*
* 
*/

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include <unistd.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "RGBImage.h"
#include "JPEGEncoder.hpp"  ///< For saving JPEG
#include "CScanMeasurementSet.h"
#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "BetBdtLocalizer.h"
#include "bbPlotUtil.h"


#define GLOBAL_MAP "Global-Map.occu" ///Our sensor map
#define MATCHED_DATA_SET "map_matched_set2.txt" ///This is the localized dataset
/**
* Globals
*/
CScanMeasurementSet *dset=NULL; ///Dataset found here
TOccupancyGrid global_map; ///Global map goes here
CBetBdtLocalizer *glocalizer; ///B&B Localizer the whole map

///Counter for plotting difficult cases into file
static int pathology_cnt=0;
bbPlotUtil *plotter;

/**
* PARAMETERS FOR TESTING
*/
static int NUM_MEASUREMENTS = 50; ///The number of consecutive scans added to localization
static int NUM_TEST_RUNS = 1;
static float odo_distance = 2.0;



float timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		//fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		if(!isStart) return fms;
		else return 0;
	}
}

void runAccuracyTest(int indx, float ax, float ay, float aa, float minSpan, float fdist,float pTh, FILE *f, FILE *f2){
	int numLeafs=0;
	CMatchStorage matches(1500);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff, odo, testPose;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	long maxNumNodes=0;
	long maxGridnodes=0;
	float sx,sy,sa;
	
	maxNumNodes = glocalizer->getMaxNodeNumberExt(global_meas,maxGridnodes,sx,sy,sa);
	float bestWeight = 0;
	float l=0;
	int i=indx;
	
	
	dset->getMeasurement(i, pos, meas); ///<Get measurement
	odo.setToDifferentialPose(pos, origin);
	l = sqrt(odo.x*odo.x+odo.y*odo.y);
	global_map.getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180.0, 0.7, meas); ///Get simulated scans
	for(int j=0;j<meas.N;j++){
		meas.a[j]-=M_PI/2.0;
	}
	//fprintf(stderr,"meas.a[0]=%f\n",meas.a[0]);
	diff.setToDifferentialPose(pos,origin);
	localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
	i++;
	for(int j=0;j<meas.N;j++){
		TMeasurementPoint mp;
		if(meas.r[j]>0){
			mp.x = meas.r[j] * cos(meas.a[j]);
			mp.y = meas.r[j] * sin(meas.a[j]);
			global_meas.addPoint(mp);
		}
	}
	
	global_meas.frame.setFrame(pos.x,pos.y,pos.a);
	bestWeight = glocalizer->computeWeight(global_meas);
	fprintf(stderr,"Best weight=%f\n",bestWeight);

	
	for(int i=0;i<NUM_TEST_RUNS*2;i++){
		float rx = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ry = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ra = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		
		///Set initial estimate randomly, but within the search volume
		testPose.set(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
		global_meas.frame.setFrame(testPose.x,testPose.y,testPose.a);
		//global_meas.frame.setFrame(origin.x,origin.y,origin.a);
		timer(true);
		glocalizer->localizeExt(global_meas,matches,numLeafs);
		float fms = timer(false);
		matches.sort();
		
		
		float dist=1000;
		int ind = 0;
		for(int j=0;j<matches.storage.size();j++){
			float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
			(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
			if(d<dist){
				dist=d;
				ind = j;
			}
		}
		
		fprintf(f,"%.2f %.2f %.2f %ld %d %.4f %.4f %.5f %ld %.3f %.3f %.3f\n",ax,ay,aa, maxNumNodes,
																															numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms);
																															
		fprintf(f2,"%.2f %.2f %.2f %.2f %.2f %.2f %.4f %d %d %.3f %d %.2f %.2f %.2f %.5f %.5f\n",origin.x,origin.y,origin.a, 
						matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
						numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
						matches.storage[ind].weight, bestWeight);
		
																															
		if(numLeafs > 100000){
			char fname[150];
			sprintf(fname,"%d_pathology_case.jpg",pathology_cnt);
			pathology_cnt++;
			plotter->setMap();
			plotter->setMeasurement(global_meas, origin, 255,0,0);
			plotter->setMeasurement(global_meas, testPose, 0,255,0);
			testPose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			plotter->setMeasurement(global_meas, testPose, 255,255,0);
			plotter->setResult(matches);
			plotter->save(fname);
		}
	}
}

////////////////////////////////////////////////////////////////////////////////////////
///ACCURACY TEST WITH REAL DATA
/////////////////////////////////////////////////////////////////////////////////////////
void runAccuracyTestReal(int indx, float ax, float ay, float aa, float minSpan, float fdist,float pTh, FILE *f, FILE *f2){
	int numLeafs=0;
	CMatchStorage matches(1500);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff, odo, testPose;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	long maxNumNodes=0;
	long maxGridnodes=0;
	float sx,sy,sa;
	
	maxNumNodes = glocalizer->getMaxNodeNumberExt(global_meas,maxGridnodes,sx,sy,sa);
	float bestWeight = 0;
	float l=0;
	int i=indx;
	
	
	dset->getMeasurement(i, pos, meas); ///<Get measurement
	odo.setToDifferentialPose(pos, origin);
	//l = sqrt(odo.x*odo.x+odo.y*odo.y);
	
	//global_map.getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180.0, 0.7, meas); ///Get simulated scans
	//for(int j=0;j<meas.N;j++){
	//	meas.a[j]-=M_PI/2.0;
	//}
	//fprintf(stderr,"meas.a[0]=%f\n",meas.a[0]);
	//diff.setToDifferentialPose(pos,origin);
	//localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
	i++;
	for(int j=0;j<meas.N;j++){
		TMeasurementPoint mp;
		if(meas.r[j]>0 && meas.r[j]<60){
			mp.x = meas.r[j] * cos(meas.a[j]);
			mp.y = meas.r[j] * sin(meas.a[j]);
			global_meas.addPoint(mp);
		}
	}
	
	global_meas.frame.setFrame(pos.x,pos.y,pos.a);
	bestWeight = glocalizer->computeWeight(global_meas);
	fprintf(stderr,"Best weight=%f\n",bestWeight);
	
	
	for(int i=0;i<NUM_TEST_RUNS*2;i++){
		float rx = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ry = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ra = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		
		///Set initial estimate randomly, but within the search volume
		testPose.set(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
		global_meas.frame.setFrame(testPose.x,testPose.y,testPose.a);
		//global_meas.frame.setFrame(origin.x,origin.y,origin.a);
		timer(true);
		glocalizer->localizeExt(global_meas,matches,numLeafs);
		float fms = timer(false);
		matches.sort();
		
		
		float dist=1000;
		int ind = 0;
		for(int j=0;j<matches.storage.size();j++){
			float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
			(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
			if(d<dist){
				dist=d;
				ind = j;
			}
		}
		
		fprintf(f,"%.2f %.2f %.2f %ld %d %.4f %.4f %.5f %ld %.3f %.3f %.3f\n",ax,ay,aa, maxNumNodes,
						numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms);
						
						fprintf(f2,"%.2f %.2f %.2f %.2f %.2f %.2f %.4f %d %d %.3f %d %.2f %.2f %.2f %.5f %.5f\n",origin.x,origin.y,origin.a, 
						matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
						numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
						matches.storage[ind].weight, bestWeight);
						
						
						if(numLeafs > 100000){
							char fname[150];
							sprintf(fname,"%d_pathology_case.jpg",pathology_cnt);
							pathology_cnt++;
							plotter->setMap();
							plotter->setMeasurement(global_meas, origin, 255,0,0);
							plotter->setMeasurement(global_meas, testPose, 0,255,0);
							testPose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
							plotter->setMeasurement(global_meas, testPose, 255,255,0);
							plotter->setResult(matches);
							plotter->save(fname);
						}
	}
}



//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
/// Run Complexity test With Virtual data
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
void runComplexityTest(int indx, float ax, float ay, float aa, float minSpan, float fdist,float pTh, FILE *f, FILE *f2){
	int numLeafs=0;
	CMatchStorage matches(1500);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff, odo, testPose;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	long maxNumNodes=0;
	long maxGridnodes=0;
	float sx,sy,sa;
	
	maxNumNodes = glocalizer->getMaxNodeNumberExt(global_meas,maxGridnodes,sx,sy,sa);
	float bestWeight = 0;
	float l=0;
	int i=indx;
	while(l<odo_distance && i<dset->size()){
		dset->getMeasurement(i, pos, meas); ///<Get measurement
		odo.setToDifferentialPose(pos, origin);
		l = sqrt(odo.x*odo.x+odo.y*odo.y);
		global_map.getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180.0, 0.7, meas); ///Get simulated scans
		for(int j=0;j<meas.N;j++){
			meas.a[j]-=M_PI/2.0;
		}
		//fprintf(stderr,"meas.a[0]=%f\n",meas.a[0]);
		diff.setToDifferentialPose(pos,origin);
		localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
		i++;
	}

	global_meas.setOccupancyGrid(&localMap, 0.65);
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	bestWeight = glocalizer->computeWeight(global_meas);
	fprintf(stderr,"Best weight=%f\n",bestWeight);

	
	for(int i=0;i<NUM_TEST_RUNS;i++){
		float rx = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ry = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ra = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		
		///Set initial estimate randomly, but within the search volume
		testPose.set(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
		global_meas.frame.setFrame(testPose.x,testPose.y,testPose.a);
		//global_meas.frame.setFrame(origin.x,origin.y,origin.a);
		timer(true);
		glocalizer->localizeExt(global_meas,matches,numLeafs);
		float fms = timer(false);
		matches.sort();
		
		
		float dist=1000;
		int ind = 0;
		for(int j=0;j<matches.storage.size();j++){
			float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
			(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
			if(d<dist){
				dist=d;
				ind = j;
			}
		}
		
		fprintf(f,"%.2f %.2f %.2f %ld %d %.4f %.4f %.5f %ld %.3f %.3f %.3f\n",ax,ay,aa, maxNumNodes,
																															numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms);
																															
		fprintf(f2,"%.2f %.2f %.2f %.2f %.2f %.2f %.4f %d %d %.3f %d %.2f %.2f %.2f %.5f %.5f %d\n",origin.x,origin.y,origin.a, 
						matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
						numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
						matches.storage[ind].weight, bestWeight, glocalizer->allLeafNodes.size());		
																															
		if(numLeafs > 100000){
			char fname[150];
			sprintf(fname,"%d_pathology_case.jpg",pathology_cnt);
			pathology_cnt++;
			plotter->setMap();
			plotter->setMeasurement(global_meas, origin, 255,0,0);
			plotter->setMeasurement(global_meas, testPose, 0,255,0);
			testPose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			plotter->setMeasurement(global_meas, testPose, 255,255,0);
			plotter->setResult(matches);
			plotter->save(fname);
		}
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Test complexity with REAL data
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void runComplexityTestReal(int indx, float ax, float ay, float aa, float minSpan, float fdist,float pTh, FILE *f, FILE *f2){
	int numLeafs=0;
	CMatchStorage matches(1500);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff, odo, testPose;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	long maxNumNodes=0;
	long maxGridnodes=0;
	float sx,sy,sa;
	
	maxNumNodes = glocalizer->getMaxNodeNumberExt(global_meas,maxGridnodes,sx,sy,sa);
	float bestWeight = 0;
	float l=0;
	int i=indx;
	while(l<odo_distance && i<dset->size()){
		dset->getMeasurement(i, pos, meas); ///<Get measurement
		odo.setToDifferentialPose(pos, origin);
		l = sqrt(odo.x*odo.x+odo.y*odo.y);
		//global_map.getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180.0, 0.7, meas); ///Get simulated scans
		//for(int j=0;j<meas.N;j++){
		//	meas.a[j]-=M_PI/2.0;
		//}
		//fprintf(stderr,"meas.a[0]=%f\n",meas.a[0]);
		diff.setToDifferentialPose(pos,origin);
		localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
		i++;
	}

	global_meas.setOccupancyGrid(&localMap, 0.65);
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	bestWeight = glocalizer->computeWeight(global_meas);
	fprintf(stderr,"Best weight=%f\n",bestWeight);

	
	for(int i=0;i<NUM_TEST_RUNS;i++){
		float rx = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ry = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		float ra = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
		
		///Set initial estimate randomly, but within the search volume
		testPose.set(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
		global_meas.frame.setFrame(testPose.x,testPose.y,testPose.a);
		timer(true);
		glocalizer->localizeExt(global_meas,matches,numLeafs);
		float fms = timer(false);
		matches.sort();
		
		float dist=1000;
		int ind = 0;
		for(int j=0;j<matches.storage.size();j++){
			float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
			(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
			if(d<dist){
				dist=d;
				ind = j;
			}
		}
		
		fprintf(f,"%.2f %.2f %.2f %ld %d %.4f %.4f %.5f %ld %.3f %.3f %.3f\n",ax,ay,aa, maxNumNodes,
																															numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms);
																															
		fprintf(f2,"%.2f %.2f %.2f %.2f %.2f %.2f %.4f %d %d %.3f %d %.2f %.2f %.2f %.5f %.5f %d\n",origin.x,origin.y,origin.a, 
						matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
						numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
						matches.storage[ind].weight, bestWeight, glocalizer->allLeafNodes.size());
		
																															
		if(numLeafs > 100000){
			char fname[150];
			sprintf(fname,"%d_pathology_case.jpg",pathology_cnt);
			pathology_cnt++;
			plotter->setMap();
			plotter->setMeasurement(global_meas, origin, 255,0,0);
			plotter->setMeasurement(global_meas, testPose, 0,255,0);
			testPose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			plotter->setMeasurement(global_meas, testPose, 255,255,0);
			plotter->setResult(matches);
			plotter->save(fname);
		}
	}
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Test JANNE
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
void runJanneTestReal(int indx, float ax, float ay, float aa, float minSpan, float fdist,float pTh, FILE *f, FILE *f2){
	int numLeafs=0;
	CMatchStorage matches(15000);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	FILE *leafFile = fopen("leafnodes.txt","wt");
	glocalizer->tuneParameters(fdist, pTh);
	glocalizer->setMinSpan(minSpan);
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff, odo, testPose;
	//#warning !!!!!!!!!!!!!!!!!!!!!!!!hardcoded index!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	//indx = 779;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	long maxNumNodes=0;
	long maxGridnodes=0;
	float sx,sy,sa;
	
	maxNumNodes = glocalizer->getMaxNodeNumberExt(global_meas,maxGridnodes,sx,sy,sa);
	float bestWeight = 0;
	float l=0;
	int i=indx;
	
	fprintf(stderr,"LODO:%.2f \n",odo_distance);
	while(l<odo_distance && i<dset->size()){
			dset->getMeasurement(i, pos, meas); ///<Get measurement
			fprintf(stderr,"IND = %d N=%d\n",i, meas.N );
			odo.setToDifferentialPose(pos, origin);
			l = sqrt(odo.x*odo.x+odo.y*odo.y);
			diff.setToDifferentialPose(pos,origin);
			localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
			i++;
	}

global_meas.setOccupancyGrid(&localMap, 0.65);
global_meas.frame.setFrame(origin.x,origin.y,origin.a);
bestWeight = glocalizer->computeWeight(global_meas);
fprintf(stderr,"Best weight=%f\n",bestWeight);


for(int i=0;i<NUM_TEST_RUNS;i++){
	float rx = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
	float ry = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
	float ra = 1.0*(((float)rand())/((float)RAND_MAX)-0.5);
	
	///Set initial estimate randomly, but within the search volume
	testPose.set(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
	global_meas.frame.setFrame(testPose.x,testPose.y,testPose.a);
	timer(true);
	glocalizer->localizeExt(global_meas,matches,numLeafs);
	float fms = timer(false);
	matches.sort();
	
	float dist=1000;
	int ind = 0;
	for(int j=0;j<matches.storage.size();j++){
		float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
		(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
		if(d<dist){
			dist=d;
			ind = j;
		}
	}
	
	fprintf(f,"%.2f %.2f %.2f %ld %d %.4f %.4f %.5f %ld %.3f %.3f %.3f\n",ax,ay,aa, maxNumNodes,
					numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms);
					
	fprintf(f2,"%.2f %.2f %.2f %.2f %.2f %.2f %.4f %d %d %.3f %d %.2f %.2f %.2f %.5f %.5f %d\n",origin.x,origin.y,origin.a, 
									matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
									numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
									matches.storage[ind].weight, bestWeight, glocalizer->allLeafNodes.size());
									
		for(int j=0;j<glocalizer->allLeafNodes.size();j++){
			fprintf(leafFile,"%.4f %.4f %.4f %.4f %.4f %.4f %.4f %d\n", glocalizer->allLeafNodes[j].cellAlignCentre.x,
							glocalizer->allLeafNodes[j].cellAlignCentre.y,
							glocalizer->allLeafNodes[j].cellAlignCentre.phii,
							glocalizer->allLeafNodes[j].cellCenterScore,
							testPose.x,testPose.y,testPose.a, indx);
		}
		
		fprintf(stderr,"Nummeas=%d\n",global_meas.size());
		
		FILE *fmeasfile = fopen("meas_file.txt","wt");
		for(int j=0; j<global_meas.size();j++){
			fprintf(fmeasfile,"%.4f %.4f\n",global_meas.points[j].x,global_meas.points[j].y);
		}
		fclose(fmeasfile);
			
		if(numLeafs > 1){
			char fname[150];
			sprintf(fname,"%d_pathology_case.jpg",pathology_cnt);
			pathology_cnt++;
			plotter->setMap();
			plotter->setMeasurement(global_meas, origin,glocalizer->grid->getResolution() , 255,0,0);
			plotter->setMeasurement(global_meas, testPose,glocalizer->grid->getResolution(), 0,255,0);
			testPose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			plotter->setMeasurement(global_meas, testPose,glocalizer->grid->getResolution(), 255,255,0);
			plotter->setResult(matches);
			plotter->save(fname);
		}
	}
	fclose(leafFile);
}













/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**
* Runs a global localization test using simulated measurement
* @param indx starting index (of real measurement set)
* @param ax Search area in x
* @param ay Search area in y
* @param aa Search area in heading
* 
**/
void runVirtualScanTest(int indx, float ax, float ay, float aa, float minSpan, float fdist){
	int numLeafs=0;
	CMatchStorage matches(150);
	TOccupancyGrid localMap(80.0, 80.0, 0.04, Grid::TCoord2D(0,0));
	localMap.setGridToValue(0.5);
	glocalizer->tuneParameters(fdist, 0.4);
	glocalizer->setMinSpan(minSpan);
	
	
	
	CEnvironmentMeasurement2D global_meas;
	global_meas.frame.setError(ax,ay,aa);		
	
	Grid::TScan meas;
	Grid::pose pos, origin, diff;
	origin = dset->poses[indx];
	
	global_meas.frame.setFrame(origin.x,origin.y,origin.a);
	//TMeasurementPoint p(100000,10000);
	//global_meas.addPoint(p);
	int maxNumNodes=0;
	//glocalizer->localizeExt(global_meas,matches,maxNumNodes);
	//fprintf(stderr,"NUMNODES=%d or %d\n",maxNumNodes,glocalizer->getMaxNodeNumber(global_meas));
	maxNumNodes = glocalizer->getMaxNodeNumber(global_meas);
	
	for(int i=indx;i<indx+NUM_MEASUREMENTS;i++){
		dset->getMeasurement(i, pos, meas); ///<Get measurement
		global_map.getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180.0, 0.7, meas); ///Get simulated scans
		for(int j=0;j<meas.N;j++){
			meas.a[j]-=M_PI/2.0;
		}
		//fprintf(stderr,"meas.a[0]=%f\n",meas.a[0]);
		diff.setToDifferentialPose(pos,origin);
		localMap.setScanFillProb(diff.x, diff.y ,diff.a, meas,0.3,0.7);
	}
	localMap.save("out.occu");
	global_meas.setOccupancyGrid(&localMap, 0.65);

	for(int i=0;i<NUM_TEST_RUNS;i++){
		float rx = 1.95*((float)rand())/((float)RAND_MAX)-1.95/2.0;
		float ry = 1.95*((float)rand())/((float)RAND_MAX)-1.95/2.0;
		float ra = 1.95*((float)rand())/((float)RAND_MAX)-1.95/2.0;
		
		///Set initial estimate randomly, but within the search volume
		global_meas.frame.setFrame(origin.x+ax*rx,origin.y+ay*ry,origin.a+aa*ra);
		//global_meas.frame.setFrame(origin.x,origin.y,origin.a);
		timer(true);
		glocalizer->localizeExt(global_meas,matches,numLeafs);
		float fms = timer(false);
		matches.sort();
		
		
		float dist=1000;
		int ind = 0;
		for(int j=0;j<matches.storage.size();j++){
			float d = sqrt( (origin.x-matches.storage[j].x)*(origin.x-matches.storage[j].x) +
							(origin.y-matches.storage[j].y)*(origin.y-matches.storage[j].y));
			if(d<dist){
				dist=d;
				ind = j;
			}
		}
		
		fprintf(stderr,"%.2f %.2f %.2f | %.2f %.2f %.2f %.4f| %d %d %.3f %d | %.2f %.2f %.2f %.4f\n",origin.x,origin.y,origin.a, 
						matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 
						numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, 
						matches.storage[ind].weight);
		
	}
}

///./generate-journal-results [folder] [fdist] [reso] [pTh] [odolength]
int main(int argc, char *argv[]){
	srand(time(NULL)); ///Initialize the random numbers
	
	dset = new CScanMeasurementSet(MATCHED_DATA_SET);
	global_map.load(GLOBAL_MAP);
	glocalizer = new CBetBdtLocalizer(GLOBAL_MAP);
	glocalizer->tuneParameters(25, 0.4);
	glocalizer->setMinSpan(0.04);
	plotter = new bbPlotUtil(glocalizer);
	
	/**
	* COMPLEXITY TEST
	*/
	float fdist = 55.0;
	float reso=0.01;
	float pTh=0.4;
	char *foldername = "default";
	
	
	if(argc>1)foldername = argv[1];
	if(argc>2)fdist = atof(argv[2]);
	if(argc>3)reso = atof(argv[3]);
	if(argc>4)pTh = atof(argv[4]);
	if(argc>5)odo_distance = atof(argv[5]);
	
	std::string cmd = "mkdir ";
	cmd+=foldername;
	system(cmd.c_str());
	chdir(foldername);
	
	
	FILE *readmefile; 
	readmefile = fopen("README.txt","wt");
	fprintf(readmefile,"Dataset for testing B&B\n");
	fprintf(readmefile,"Logfile contents: \n");
	fprintf(readmefile,"*_complexity_test.txt: ax,ay,aa, maxNumNodes,numLeafs,sx,sy,sa,maxGridnodes, minSpan, fdist,fms\n");
	fprintf(readmefile,"*_accuracy_test.txt: origin.x,origin.y,origin.a, matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,matches.storage[0].weight, maxNumNodes, 	numLeafs, fms, ind, matches.storage[ind].x,matches.storage[ind].y,matches.storage[ind].a, matches.storage[ind].weight\n");
	fprintf(readmefile,"Parameters:\n");
	fprintf(readmefile,"fdist:%.4f\n",fdist);
	fprintf(readmefile,"reso:%.4f\n",reso);
	fprintf(readmefile,"pTh:%.4f\n",pTh);
	fprintf(readmefile,"odo_distance:%.4f\n",odo_distance);
	fclose(readmefile);	
	
/////////////////////////
///Test accuracy VIRTUAL
/////////////////////////
/*	float ax=0.5;
	float ay=0.5; 
	float aa = M_PI/4;
	
	for(int i=0;i<20;i++){
	 char filename[150];
	 sprintf(filename,"%d_complexity_test.txt",i);
	 FILE *fout = fopen(filename,"wt");
	 sprintf(filename,"%d_accuracy_test.txt",i);
	 FILE *facc =  fopen(filename,"wt");
	 
	 fprintf(stderr,"Processing: %s\n",filename);
	 
		for(int ii=0;ii<20;ii++){
			int indx =(int) ((float)dset->size()*((float)rand()/(float)RAND_MAX)) - NUM_MEASUREMENTS;
			if(indx<0) indx+=NUM_MEASUREMENTS;
			runAccuracyTest(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
		}
		fclose(fout);
		fclose(facc);
	}
	return 0;
	*/
	/////////////////////////
	///Test accuracy REAL
	/////////////////////////
	/*
	float ax=0.5;
	float ay=0.5; 
	float aa = M_PI/4;
	
	for(int i=0;i<20;i++){
		char filename[150];
		sprintf(filename,"%d_complexity_test.txt",i);
		FILE *fout = fopen(filename,"wt");
		sprintf(filename,"%d_accuracy_test.txt",i);
		FILE *facc =  fopen(filename,"wt");
		
		fprintf(stderr,"Processing: %s\n",filename);
		
		for(int ii=0;ii<20;ii++){
			int indx =(int) ((float)dset->size()*((float)rand()/(float)RAND_MAX)) - NUM_MEASUREMENTS;
			if(indx<0) indx+=NUM_MEASUREMENTS;
			runAccuracyTestReal(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
		}
		fclose(fout);
		fclose(facc);
	}
	return 0;	
*/	
	
	/*
/////////////////////////////////////
///Test area VIRTUAL
///////////////////////////////////
	float ax=0.1;
	float ay=0.1; 
	float aa = M_PI/2;
	for(int i=0;i<20;i++){
	 
	////
	 // Run the test N times
	 ////
	 char filename[150];
	 sprintf(filename,"%d_complexity_test.txt",i);
	 FILE *fout = fopen(filename,"wt");
	 sprintf(filename,"%d_accuracy_test.txt",i);
	 FILE *facc =  fopen(filename,"wt");
	 
	 fprintf(stderr,"Processing: %s\n",filename);
	 
		for(int ii=0;ii<20;ii++){
			int indx =(int) ((float)dset->size()*((float)rand()/(float)RAND_MAX)) - NUM_MEASUREMENTS;
			if(indx<0) indx+=NUM_MEASUREMENTS;
			//runAccuracyTest(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
	
			runComplexityTest(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
		}
		fclose(fout);
		fclose(facc);
		ax+=.5;
		ay+=.5;
		//aa*=1.5;
	}
*/
	
/////////////////////////////////////
///Test area REAL
///////////////////////////////////
// 	float ax=0.1;
// 	float ay=0.1; 
// 	float aa = M_PI/2;
// 	for(int i=0;i<20;i++){
// 	 
// 	////
// 	 /// Run the test N times
// 	 ////
// 	 char filename[150];
// 	 sprintf(filename,"%d_complexity_test.txt",i);
// 	 FILE *fout = fopen(filename,"wt");
// 	 sprintf(filename,"%d_accuracy_test.txt",i);
// 	 FILE *facc =  fopen(filename,"wt");
// 	 
// 	 fprintf(stderr,"Processing: %s\n",filename);
// 	 
// 		for(int ii=0;ii<20;ii++){
// 			int indx =(int) ((float)dset->size()*((float)rand()/(float)RAND_MAX)) - NUM_MEASUREMENTS;
// 			if(indx<0) indx+=NUM_MEASUREMENTS;
// 			//runAccuracyTest(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
// 			runComplexityTestReal(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);
// 		}
// 		fclose(fout);
// 		fclose(facc);
// 		ax+=0.5;
// 		ay+=0.5;
// 		//aa*=1.5;
// 	}
	
	/////////////////////////////////////
	///TEST JANNE
	///////////////////////////////////
		float ax=15.0;
		float ay=15.0; 
		float aa = M_PI;
		////
		/// Run the test N times
		////
		char filename[150];
		sprintf(filename,"%d_complexity_test.txt",0);
		FILE *fout = fopen(filename,"wt");
		sprintf(filename,"%d_accuracy_test.txt",0);
		FILE *facc =  fopen(filename,"wt");
		
		fprintf(stderr,"Processing: %s\n",filename);
		
		int indx =(int) ((float)dset->size()*((float)rand()/(float)RAND_MAX)) - NUM_MEASUREMENTS;
		if(indx<0) indx+=NUM_MEASUREMENTS;
		runJanneTestReal(indx, ax, ay, aa, reso, fdist,pTh, fout, facc);

		fclose(fout);
		fclose(facc);
	
	
	
	////////////////////////////////////////////////////////////////////
	////////////////////////////////////////////////////////////////////
	////////////////////////////////////////////////////////////////////
	/*
	runVirtualScanTest(402,2.0, 2.0, M_PI/4, 0.01,35.0);
	*/
	
	return 0;
}

